
#include "p24HJ128GP504.h"
#include "input.h"
#include "gui2.h"
#include "oled.h"
#include "button.h"
#include "delay.h"
#include "stepper.h"
#include "icons.h"
#include "adc_sensors.h"
#include "bike.h"

// Menu control variables
extern JoyStick joyStick;
extern Menu mainMenu;
extern Menu modeCfgMenu;
extern Menu sagCfgMenu;
extern unsigned char currentMode;

// Analogue to digital variables
extern AdcBuf adcBufA;

// Variables to control the gui configurable motor settings
unsigned int xcCompression = 67;
unsigned int xcRebound = 396;

unsigned int amCompression = 45;
unsigned int amRebound = 804;

unsigned int dhCompression = 17;
unsigned int dhRebound = 1200;

unsigned int rdCompression = 89;
unsigned int rdRebound = 204;

unsigned int rcCompression = 111;
unsigned int rcRebound = 804;

// Debug flag settable from the gui
unsigned int impactDebug = 0;

// current motor setting as determined by the gui
unsigned int *modeCompressionPosition = &xcCompression;
unsigned int *modeReboundPosition = &xcRebound;

// Sag configure variables
extern unsigned int weight;
extern unsigned int suggestedPsi;
extern unsigned int currentSag;
extern unsigned char currentSagItem;

// motor position variables
extern motor compression;
extern motor rebound;

// inclinometer variables
extern unsigned int windowAngle[128];
extern unsigned char angleNegativeFlag[128];

extern unsigned char statHandlerFlag;

// Simple graphical up and down arrows
unsigned int upArrow[5*3] = 
{
    BLUE,BLUE,YELLOW,BLUE,BLUE,
    BLUE,YELLOW,YELLOW,YELLOW,BLUE,
    YELLOW,YELLOW,YELLOW,YELLOW,YELLOW
};
unsigned int downArrow[5*3] = 
{
    YELLOW,YELLOW,YELLOW,YELLOW,YELLOW,
    BLUE,YELLOW,YELLOW,YELLOW,BLUE,
    BLUE,BLUE,YELLOW,BLUE,BLUE,
};

unsigned int upArrowBlack[5*3] = 
{
    BLUE,BLUE,BLACK,BLUE,BLUE,
    BLUE,BLACK,BLACK,BLACK,BLUE,
    BLACK,BLACK,BLACK,BLACK,BLACK
};

unsigned int downArrowBlack[5*3] = 
{
    BLACK,BLACK,BLACK,BLACK,BLACK,
    BLUE,BLACK,BLACK,BLACK,BLUE,
    BLUE,BLUE,BLACK,BLUE,BLUE,
};

unsigned int upArrowGrey[5*3] = 
{
    BLUE,BLUE,GREY,BLUE,BLUE,
    BLUE,GREY,GREY,GREY,BLUE,
    GREY,GREY,GREY,GREY,GREY
};

unsigned int downArrowGrey[5*3] = 
{
    GREY,GREY,GREY,GREY,GREY,
    BLUE,GREY,GREY,GREY,BLUE,
    BLUE,BLUE,GREY,BLUE,BLUE,
};  

void initGui( void )
{
    // init beeperx
    TRISAbits.TRISA1=0;
    LATAbits.LATA1=0;
    
    // init joystick
    UP_TRIS = 1;
    DOWN_TRIS = 1;
    LEFT_TRIS = 1;
    RIGHT_TRIS = 1;
    SELECT_TRIS = 1;
    
    initTimer1(1000);
      
    joyStick.enabled = 1;
    
    displayMainMenu(); 
}

void displayMenuItem( MenuItem *menuItem )
{
    

    // write background
    writeImage( menuItem->button.background.x,
                menuItem->button.background.y,
                menuItem->button.background.graphic,
                menuItem->button.background.width,
                menuItem->button.background.height
              );
    // write icon
    writeImage( menuItem->button.icon.x,
                menuItem->button.icon.y,
                menuItem->button.icon.graphic,
                menuItem->button.icon.width,
                menuItem->button.icon.height
              );
    // write text
    writeString( menuItem->button.text.x,
                 menuItem->button.text.y,
                 menuItem->button.text.text,
                 menuItem->button.text.length,
                 menuItem->button.text.colourFore,
                 menuItem->button.text.colourBack,
                 menuItem->button.text.colourShadow
               );
};    

void displayMainMenu( void )
{
    unsigned int i;
    
    clearOled(BLUE);
    
    for( i = 0; i < 8; i ++ )
    {
        displayMenuItem(&(mainMenu.menuItems[i]));
    }
    
    writeString(45, 46 , "MODES", 5, BLACK, BLUE, BORDER_BLUE);        
}
void displayModeCfgSubMenu( TextObject *title, GraphicObject *icon)
{
    unsigned int i;
    
    clearOled(BLUE);
    
    for( i = 0; i < 3; i ++ )
    {
        displayMenuItem(&( modeCfgMenu.menuItems[i]));
    }
    
    // write icon
    writeImage( icon->x,
                icon->y,
                icon->graphic,
                icon->width,
                icon->height
              );
    // write text
    writeString( title->x,
                 title->y,
                 title->text,
                 title->length,
                 title->colourFore,
                 title->colourBack,
                 title->colourShadow
               );
   writeImage(47, 44, upArrowBlack,   5, 3);
   writeImage(47, 50, downArrowBlack, 5, 3);
   writeString(54,44,"CMP", 3, BLACK, BLUE, BORDER_BLUE);
   writeImage(91, 44, upArrowBlack,   5, 3);
   writeImage(91, 50, downArrowBlack, 5, 3);
   writeString(98,44,"RBD", 3, BLACK, BLUE, BORDER_BLUE);
}    

void displaySagCfgSubMenu( unsigned int sugPsi,
                           unsigned int curSag)
{
    char string[7];
    unsigned int i;
    for( i = 0; i <2; i++ )
    {
        displayMenuItem(&( sagCfgMenu.menuItems[i]));
    }
    
    my_itoa(sugPsi, string);
    stringCopy("PSI", &string[4], 3);
    writeString(127-55, 27, string, 7, GREEN, BLUE, BORDER_BLUE);
}    

void updateMainMenu( void )
{
   unsigned char newMenu; 
   switch( joyStick.theStick )
   {
        case UP:
            newMenu = mainMenu.menuNavigation[mainMenu.currentMenuItem][0];
            changeSelection( &(mainMenu.menuItems[mainMenu.currentMenuItem]), 
                             &(mainMenu.menuItems[newMenu]));
            mainMenu.currentMenuItem = newMenu;
        break;
        case DOWN:
            newMenu = mainMenu.menuNavigation[mainMenu.currentMenuItem][1];
            changeSelection( &(mainMenu.menuItems[mainMenu.currentMenuItem]), 
                             &(mainMenu.menuItems[newMenu]));
            mainMenu.currentMenuItem = newMenu;
        break;
        case LEFT:
            newMenu = mainMenu.menuNavigation[mainMenu.currentMenuItem][2];
            changeSelection( &(mainMenu.menuItems[mainMenu.currentMenuItem]), 
                             &(mainMenu.menuItems[newMenu]));
            mainMenu.currentMenuItem = newMenu;
        break;
        case RIGHT:
            newMenu = mainMenu.menuNavigation[mainMenu.currentMenuItem][3];
            changeSelection( &(mainMenu.menuItems[mainMenu.currentMenuItem]), 
                             &(mainMenu.menuItems[newMenu]));
            mainMenu.currentMenuItem = newMenu;
        break;  
        case SELECT:
            mainMenu.menuItems[mainMenu.currentMenuItem].selectHandler();
        default:
        break;  
   }
}  
       
void changeSelection( MenuItem *deselect, MenuItem *select )
{
    switch( deselect->button.buttonType )
    {
        case 0: // big button
            switch( select->button.buttonType )
            {
                case 0: // big button
                    deselect->button.background.graphic = big_button_unselected;
                    select->button.background.graphic = big_button_selected;
                break;
                case 1: // mode button
                    deselect->button.background.graphic = big_button_unselected;
                    if( select->isHighlighted )
                        select->button.background.graphic = mode_button_highlighted_selected;
                    else
                        select->button.background.graphic = mode_button_unhighlighted_selected;
                    
                break;
            }
        break;    
        case 1: // mode button
            switch( select->button.buttonType )
            {
                case 0: // big button
                    if( deselect->isHighlighted )
                        deselect->button.background.graphic = mode_button_highlighted_unselected;
                    else
                        deselect->button.background.graphic = mode_button_unhighlighted_unselected;   
                    select->button.background.graphic = big_button_selected;
                break;
                case 1: // mode button
                    if( deselect->isHighlighted )
                        deselect->button.background.graphic = mode_button_highlighted_unselected;
                    else
                        deselect->button.background.graphic = mode_button_unhighlighted_unselected; 
                         
                    if( select->isHighlighted )
                        select->button.background.graphic = mode_button_highlighted_selected;
                    else
                        select->button.background.graphic = mode_button_unhighlighted_selected;   
                break;
            }
        break;
    } 
      
    displayMenuItem( deselect );
    displayMenuItem( select );
}

void statHandler( void )
{
   unsigned int tempImpact = 0;
   unsigned int tempXaxis = 0;
   unsigned int tempYaxis = 0;
   unsigned int tempPot = 0;
   unsigned int tempAngle = 0;
   unsigned int i;
   unsigned char string[3];
   static unsigned int maxImpact = 0;
   
   if( statHandlerFlag == 0 )
   {
       clearOled(BLUE);
       writeString(43, 2, "STATUS", 6, BLACK, BLUE, BORDER_BLUE);
       writeImage( 50, 16, bike, 27, 15 );
       writeString(2, 27, "IMPACT", 6, YELLOW, BLUE, BORDER_BLUE);
       writeString(2, 39, "X AXIS", 6, YELLOW, BLUE, BORDER_BLUE);
       writeString(2, 51, "Y AXIS", 6, YELLOW, BLUE, BORDER_BLUE);
       writeString(2, 63, "ANGLE", 5, YELLOW, BLUE, BORDER_BLUE);
       writeString(2, 75, "POT", 3, YELLOW, BLUE, BORDER_BLUE);
       writeString(2, 88, "COMP", 4, YELLOW, BLUE, BORDER_BLUE);
       writeString(2, 88, "COMP", 4, YELLOW, BLUE, BORDER_BLUE);
   } 
  
   
   statHandlerFlag = 1;
   
   //while( 1 )
   {
        tempImpact = 0;
        tempXaxis = 0;
        tempYaxis = 0;
        tempPot = 0;
        tempAngle = 0;
        for( i = 0; i < 32; i++)
        {
            //tempImpact += adcBufA.impact[i];
            tempXaxis += adcBufA.xaxis[i];
            tempYaxis += adcBufA.yaxis[i];
            tempPot += adcBufA.pot[i];
            if( adcBufA.impact[i] > maxImpact )
            {
                maxImpact = adcBufA.impact[i];
            }    
        }       
              
        tempImpact = maxImpact;
        my_itoa(tempImpact, string);
        writeString(127-23, 27, string, 3, YELLOW, BLUE, BORDER_BLUE);
        
        tempXaxis /= 32;
        my_itoa(tempXaxis, string);
        writeString(127-23, 39, string, 3, YELLOW, BLUE, BORDER_BLUE);
        
        tempYaxis /= 32;
        my_itoa(tempYaxis, string);
        writeString(127-23, 51, string, 3, YELLOW, BLUE, BORDER_BLUE);
        
        tempAngle = windowAngle[0];
        my_itoa(tempAngle, string);
        writeString(127-23, 63, string, 3, YELLOW, BLUE, BORDER_BLUE);
        if( angleNegativeFlag[0] )
        {
            writeString(44, 63, "+", 1, YELLOW, BLUE, BORDER_BLUE);
        }
        else
        {
            writeString(44, 63, "-", 1, YELLOW, BLUE, BORDER_BLUE);   
        }    
        
        tempPot /= 32;
        my_itoa(tempPot, string);
        writeString(127-23, 75, string, 3, YELLOW, BLUE, BORDER_BLUE);
    
        my_itoa(compression.currentPosition, string);        
        writeString(127-23, 88, string, 3, YELLOW, BLUE, BORDER_BLUE);
        
        my_itoa(rebound.currentPosition, string);
        writeString(127-23, 101, string, 3, YELLOW, BLUE, BORDER_BLUE);
        
        if( impactDebug )
        {
            writeString(2, 127-11, "IMPACT DBG ON ", 14, RED, BLUE, BORDER_BLUE);
        }
        else
        {
            writeString(2, 127-11, "IMPACT DBG OFF", 14, GREY, BLUE, BORDER_BLUE);
        }        
        
        if( joyStick.enabled )
        {
            if( joyStick.pressed )
            {
                LATAbits.LATA1 = 1;
                delayMs(20);
                LATAbits.LATA1 = 0;
                joyStick.pressed = 0;
                joyStick.enabled = 0;
                if(joyStick.select)
                {
                    statHandlerFlag = 0;
                    displayMainMenu();  
                }
                else if (joyStick.left)
                {
                    writeString(2, 127-11, "IMPACT DBG OFF", 14, GREY, BLUE, BORDER_BLUE);
                    impactDebug = 0;                    
                }    
                else if (joyStick.right)
                {
                    writeString(2, 127-11, "IMPACT DBG ON ", 14, RED, BLUE, BORDER_BLUE);                    
                    impactDebug = 1;
                }    
            }     
        }
    }    
}    
void sagCfgHandler( void )
{
    sagSubMenu();
    displayMainMenu();   
}    

void shutDownHandler( void )
{
   sleepOled();
   while( 1 )
   {
        if( joyStick.enabled )
        {
            if( joyStick.pressed )
            {
                LATAbits.LATA1 = 1;
                delayMs(20);
                LATAbits.LATA1 = 0;
                joyStick.pressed = 0;
                joyStick.enabled = 0;
                if(joyStick.select)
                {
                    wakeUpOled();
                    break;  
                }    
            }     
        }
    }
    displayMainMenu();
}    

void modeCfgHandler( void )
{
    if( currentMode != mainMenu.currentMenuItem )
    {
        mainMenu.menuItems[currentMode].button.background.graphic = mode_button_unhighlighted_unselected;
        mainMenu.menuItems[currentMode].isHighlighted = 0;
        switch( currentMode )
        {
            case 3:
                mainMenu.menuItems[currentMode].button.icon.graphic = xc;
            break;
            case 4:
                mainMenu.menuItems[currentMode].button.icon.graphic = am;
            break;
            case 5:
                mainMenu.menuItems[currentMode].button.icon.graphic = dh;
            break;
            case 6:
                mainMenu.menuItems[currentMode].button.icon.graphic = rd;
            break;
            case 7:
                mainMenu.menuItems[currentMode].button.icon.graphic = rc;
            break;   
        }    
        displayMenuItem( &mainMenu.menuItems[currentMode] );
        
        currentMode = mainMenu.currentMenuItem;
        mainMenu.menuItems[currentMode].isHighlighted = 1;
        mainMenu.menuItems[currentMode].button.background.graphic = mode_button_highlighted_selected;
        switch( currentMode )
        {
            case 3:
                mainMenu.menuItems[currentMode].button.icon.graphic = xc_sel;
                modeCompressionPosition = &xcCompression;
                modeReboundPosition = &xcRebound;
            break;
            case 4:
                mainMenu.menuItems[currentMode].button.icon.graphic = am_sel;
                modeCompressionPosition = &amCompression;
                modeReboundPosition = &amRebound;
            break;
            case 5:
                mainMenu.menuItems[currentMode].button.icon.graphic = dh_sel;
                modeCompressionPosition = &dhCompression;
                modeReboundPosition = &dhRebound;
            break;
            case 6:
                mainMenu.menuItems[currentMode].button.icon.graphic = rd_sel;
                modeCompressionPosition = &rdCompression;
                modeReboundPosition = &rdRebound;
            break;
            case 7:
                mainMenu.menuItems[currentMode].button.icon.graphic = rc_sel;
                modeCompressionPosition = &rcCompression;
                modeReboundPosition = &rcRebound;
            break;   
        }
        setCompressionPosition(*modeCompressionPosition);
        setReboundPosition(*modeReboundPosition);
        displayMenuItem( &mainMenu.menuItems[currentMode] );
        
    }
    else
    {
        modeSubMenu();
        displayMainMenu();   
    }        
}

void modeSubMenu( void )
{
    TextObject title;
    GraphicObject icon;
      
    char string[3];
    
    title.colourFore = BLACK;
    title.colourBack = BLUE;
    title.colourShadow = BORDER_BLUE;
    
    switch( currentMode )
    {
        case 3:
            title.x = 13;
            title.y = 2;
            stringCopy("CROSS COUNTRY", title.text, 13);
            title.length = 13;
            icon.x = 50;
            icon.y = 16;
            icon.graphic = xc; 
            icon.width = 32;
            icon.height = 14;
            modeReboundPosition = &xcRebound;
            modeCompressionPosition = &xcCompression;
        break;
        case 4:
            title.x = 16;
            title.y = 2;
            stringCopy("ALL MOUNTAIN", title.text, 12);
            title.length = 12;
            icon.x = 50;
            icon.y = 16;
            icon.graphic = am; 
            icon.width = 30;
            icon.height = 14;
            modeReboundPosition = &amRebound;
            modeCompressionPosition = &amCompression;
        break;
        case 5:
            title.x = 32;
            title.y = 2;
            stringCopy("DOWN HILL", title.text, 9);
            title.length = 9;
            icon.x = 50;
            icon.y = 16;
            icon.graphic = dh; 
            icon.width = 31;
            icon.height = 14;
            modeReboundPosition = &dhRebound;
            modeCompressionPosition = &dhCompression;
        break;
        case 6:
            title.x = 50;
            title.y = 2;
            stringCopy("ROAD", title.text, 4);
            title.length = 4;
            icon.x = 50;
            icon.y = 16;
            icon.graphic = rd; 
            icon.width = 30;
            icon.height = 14;
            modeReboundPosition = &rdRebound;
            modeCompressionPosition = &rdCompression;
        break;
        case 7:
            title.x = 50;
            title.y = 2;
            stringCopy("RACE", title.text, 4);
            title.length = 4;
            icon.x = 50;
            icon.y = 16;
            icon.graphic = rc; 
            icon.width = 30;
            icon.height = 14;
            modeReboundPosition = &rcRebound;
            modeCompressionPosition = &rcCompression;    
        break;
    }
    my_itoa(*modeCompressionPosition/COMPRESSION_STEPS_TO_PRECENT, string);
    stringCopy(string, modeCfgMenu.menuItems[1].button.text.text, 3);
    my_itoa(*modeReboundPosition/REBOUND_STEPS_TO_PERCENT, string);
    stringCopy(string, modeCfgMenu.menuItems[2].button.text.text, 3);
 
    displayModeCfgSubMenu( &title, &icon );
    while( 1 )
    {
        if( joyStick.enabled )
        {
            if( joyStick.pressed)
            {
                LATAbits.LATA1=1;
                delayMs(20);
                LATAbits.LATA1=0;
                joyStick.pressed = 0;
                joyStick.enabled = 0;
                if (updateModeCfgMenu(modeCompressionPosition, modeReboundPosition) )
                {
                    break;
                }    
            }     
        }
    }    
}     

void sagSubMenu( void )
{ 
    //icon.x = 50;
    //icon.y = 16;
    //icon.graphic = shock; 
    //icon.width = 27;
    //icon.height = 15;
   unsigned int i;
   unsigned char string[6];
   clearOled(BLUE);

   writeString( 31, 2, "SAG SETUP", 9, BLACK, BLUE, BORDER_BLUE);
   writeString(2,16,"SGST PSI", 8, BLACK, BLUE, BORDER_BLUE);
   writeString(2,40,"MEAS SAG",8, BLACK, BLUE, BORDER_BLUE);
   writeString(2,64,"REC SAG", 7, BLACK, BLUE, BORDER_BLUE);
   writeString(127-64, 75, "25-35 MM", 8, BLACK, BLUE, BORDER_BLUE);
   writeString(2,88,"WEIGHT", 6, BLACK, BLUE, BORDER_BLUE);


   currentSagItem = 0;
   sagCfgMenu.menuItems[0].button.text.colourFore = YELLOW;
   writeImage( 127-63, 99, upArrow, 5, 3); 
   writeImage( 127-63, 105, downArrow, 5, 3); 
   sagCfgMenu.menuItems[1].button.text.colourFore = GREY;
   suggestedPsi = getSuggestedPsi(weight);
   displaySagCfgSubMenu(suggestedPsi, currentSag);
    while( 1 )
    {
        currentSag = 0;
        for( i = 0; i < 32; i++)
        {
            currentSag += adcBufA.pot[i];
        }       
        currentSag = ((currentSag/32) - 658) /1.82000 ;
        if(currentSag < 0)
            currentSag = 0;
            
        my_itoa(currentSag, string);
        stringCopy(" MM", &string[3], 3);
        if( currentSag >= 25 && currentSag <=35 )
        {
            writeString(127-48, 51, string, 6, GREEN, BLUE, BORDER_BLUE);
        }    
        else
        {
            writeString(127-48, 51, string, 6, RED, BLUE, BORDER_BLUE);
        }
            
        if( joyStick.enabled )
        {
            if( joyStick.pressed)
            {
                LATAbits.LATA1 = 1;
                delayMs(20);
                LATAbits.LATA1=0;
                joyStick.pressed = 0;
                joyStick.enabled = 0;
                if (updateSagCfgMenu() )
                {
                    break;
                }    
            }     
        }
    }
}    

unsigned char updateSagCfgMenu( void )
{
    unsigned char string[3];
    switch( joyStick.theStick )
    {
        case UP:
            if(currentSagItem == 0)
            {
                if(weight < 250)
                {
                    weight +=5;
                    my_itoa(weight, string);
                    stringCopy(string, &sagCfgMenu.menuItems[0].button.text.text[0], 3);
                    suggestedPsi = getSuggestedPsi(weight);
                    displaySagCfgSubMenu(suggestedPsi, currentSag);
                }    
            }                
        break;
        case DOWN:
            if(currentSagItem == 0)
            {
                if(weight > 100)
                {
                    weight -=5;
                    my_itoa(weight, string);
                    stringCopy(string, &sagCfgMenu.menuItems[0].button.text.text[0], 3);
                    suggestedPsi = getSuggestedPsi(weight);
                    displaySagCfgSubMenu(suggestedPsi, currentSag);
                }    
            }
        break;
        case LEFT:
            if(currentSagItem == 0)
            {
                currentSagItem = 1;
                sagCfgMenu.menuItems[0].button.text.colourFore = GREY;
                sagCfgMenu.menuItems[1].button.text.colourFore = YELLOW;
                displaySagCfgSubMenu(suggestedPsi, currentSag);
                writeImage( 127-63, 99, upArrowGrey, 5, 3); 
                writeImage( 127-63, 105, downArrowGrey, 5, 3); 
            }
        break;
        case RIGHT:
            if(currentSagItem == 1)
            {
                currentSagItem = 0;
                sagCfgMenu.menuItems[0].button.text.colourFore = YELLOW;
                sagCfgMenu.menuItems[1].button.text.colourFore = GREY;
                displaySagCfgSubMenu(suggestedPsi, currentSag);
                writeImage( 127-63, 99, upArrow, 5, 3); 
                writeImage( 127-63, 105, downArrow, 5, 3); 
            }
        break;
        case SELECT:
            if( currentSagItem == 1)
            {
                return 1;
            }    
        break;
    }    
    return 0;
}    

unsigned char updateModeCfgMenu(unsigned int *lcompression, unsigned int *lrebound)
{
    unsigned char newMenu;
    char string[3];
    switch( joyStick.theStick )
    {
        case UP:
            if( modeCfgMenu.currentMenuItem == 1 )
            {
                if(*lcompression < MAX_COMPRESSION_STEPS)
                {
                    (*lcompression) += COMPRESSION_STEP;
                    // Hack to get around the fact that MAX_COMPRESSION_STEPS not divisible by 100
                    // and to make sure we get the divisor, really should add the mod to a counter 
                    // and when it is greater than ten counter -= counter increment by step
                    if(*lcompression % 10 == 0 && *lcompression < MAX_COMPRESSION_STEPS )
                    {
                        (*lcompression) += COMPRESSION_STEP;
                        stepCompression( COMPRESSION_STEP, CCW );
                    }                     
                    stepCompression( COMPRESSION_STEP, CCW );
                }    
                my_itoa( (unsigned char)(*lcompression/COMPRESSION_STEPS_TO_PRECENT ), string );
                modeCfgMenu.menuItems[1].button.text.text[0] = string[0];
                modeCfgMenu.menuItems[1].button.text.text[1] = string[1];
                modeCfgMenu.menuItems[1].button.text.text[2] = string[2];
                displayMenuItem(&modeCfgMenu.menuItems[1]);
            }    
            else if ( modeCfgMenu.currentMenuItem == 2 )
            {
                if( *lrebound < MAX_REBOUND_STEPS )
                {
                    *lrebound += REBOUND_STEP;
                    stepRebound( REBOUND_STEP, CCW );
                }    
                my_itoa( (*lrebound)/REBOUND_STEPS_TO_PERCENT, string );
                modeCfgMenu.menuItems[2].button.text.text[0] = string[0];
                modeCfgMenu.menuItems[2].button.text.text[1] = string[1];
                modeCfgMenu.menuItems[2].button.text.text[2] = string[2];
                displayMenuItem(&modeCfgMenu.menuItems[2]);
            }    
        break;
        case DOWN:
            if( modeCfgMenu.currentMenuItem == 1 )
            {
                if( *lcompression > 0 )
                {
                    (*lcompression) -=COMPRESSION_STEP;
                    if(*lcompression % 10 == 0 && *lcompression != 0)
                    {
                        (*lcompression) -= COMPRESSION_STEP; 
                        stepCompression( COMPRESSION_STEP, CW );  
                    }                     
                    stepCompression( COMPRESSION_STEP, CW );
                }  
                my_itoa( (unsigned char)(*lcompression/COMPRESSION_STEPS_TO_PRECENT), string );
                modeCfgMenu.menuItems[1].button.text.text[0] = string[0];
                modeCfgMenu.menuItems[1].button.text.text[1] = string[1];
                modeCfgMenu.menuItems[1].button.text.text[2] = string[2];
                displayMenuItem(&modeCfgMenu.menuItems[1]);
            }    
            else if ( modeCfgMenu.currentMenuItem == 2 )
            {
                if( *lrebound > 0 )
                {
                    *lrebound -= REBOUND_STEP;
                    stepRebound( REBOUND_STEP, CW );
                }  
                my_itoa( (*lrebound)/REBOUND_STEPS_TO_PERCENT, string );
                modeCfgMenu.menuItems[2].button.text.text[0] = string[0];
                modeCfgMenu.menuItems[2].button.text.text[1] = string[1];
                modeCfgMenu.menuItems[2].button.text.text[2] = string[2];
                displayMenuItem(&modeCfgMenu.menuItems[2]);
            }    
        break;
        case LEFT:
            newMenu = modeCfgMenu.menuNavigation[modeCfgMenu.currentMenuItem][2];
            changeSelection( &(modeCfgMenu.menuItems[modeCfgMenu.currentMenuItem]), 
                         &(modeCfgMenu.menuItems[newMenu]));
            modeCfgMenu.currentMenuItem = newMenu;
        break;
        case RIGHT:
            newMenu = modeCfgMenu.menuNavigation[modeCfgMenu.currentMenuItem][3];
            changeSelection( &(modeCfgMenu.menuItems[modeCfgMenu.currentMenuItem]), 
                         &(modeCfgMenu.menuItems[newMenu]));
            modeCfgMenu.currentMenuItem = newMenu;
        case SELECT:
        if( modeCfgMenu.currentMenuItem == 0 )
            return 1;
        break;
    }
    
    return 0;
}    

void my_itoa( unsigned int n, char *s )
{
    unsigned char i = 2;
    
    do
    {
        s[i] = n % 10 + '0';
        i--;
    }while( (n/=10) >= 1 );
    if( i == 1 )
    {
        s[0] = ' ';
        s[1] = ' ';
    }
    else if( i == 0 )
    {
        s[0] = ' ';   
    }      
}  

void stringCopy( const char *string, char *s, unsigned char length )
{
    unsigned char i;
    
    for( i = 0; i < length; i++ )
    {
        s[i] = string[i];   
    }        
}    

unsigned int getSuggestedPsi( unsigned int weight )
{
       switch(weight)
       {
           case 100:
               return 50;
           break;
           case 105:
               return 53;
           break;
           case 110:
               return 55;
           break;
           case 115:
               return 58;
           break;
           case 120:
               return 60;
           break;
           case 125:
               return 63;
           break;
           case 130:
               return 65;
           break;
           case 135:
               return 68;
           break;
           case 140:
               return 70;
           break;
           case 145:
               return 74;
           break;
           case 150:
               return 78;
           break;
           case 155:
               return 81;
           break;
           case 160:
               return 85;
           break;
           case 165:
               return 89;
           break;
           case 170:
               return 93;
           break;
           case 175:
               return 96;
           break;
           case 180:
               return 100;
           break;
           case 185:
               return 105;
           break;
           case 190:
               return 110;
           break;
           case 195:
               return 115;
           break;
           case 200:
               return 120;
           break;
           case 205:
               return 125;
           break;
           case 210:
               return 130;
           break;
           case 215:
               return 135;
           break;
           case 220:
               return 140;
           break;
           case 225:
               return 145;
           break;
           case 230:
               return 150;
           break;
           case 235:
               return 155;
           break;
           case 240:
               return 160;
           break;
           case 245:
               return 165;
           break;
           case 250:
               return 170;
           break;
           
       }
       return 0;    
}    
